#include "ParticlesData.h"
#include <vector>
#include <cmath>
#include <fstream>

ParticlesData::ParticlesData()
{
	
}


ParticlesData::~ParticlesData()
{
}


void ParticlesData::DisplayParticleWeight()
{
	for (auto& p : m_particles)
		cout << p.log_w << " ";
	cout << endl;
}

double ParticlesData::GetSumOfParticle()
{
	double  sum{.0};
	for (auto& p : m_particles)
		sum += p.log_w;
	return sum;
}

double ParticlesData::getMaxWeight()
{
	double  max_weight{ .0 };
	for (auto& p : m_particles)
		max_weight = std::max<double>(max_weight,p.log_w);
	return max_weight;
}

Particle<Pose3D>* ParticlesData::getMostLikelyParticle()
{
	// TODO
	return nullptr;
}

std::tuple<Eigen::MatrixXd, Pose3D> ParticlesData::getCovarianceAndMean()
{
	Eigen::MatrixXd cov= Eigen::MatrixXd::Zero(6, 6);
	Pose3D mean;
	getMean(mean);
	Eigen::VectorXd vars = Eigen::VectorXd::Zero(6);

	// Elements off the diagonal of the covariance matrix:
	double std_xy = 0, std_xz = 0, std_xya = 0, std_xp = 0, std_xr = 0;
	double std_yz = 0, std_yya = 0, std_yp = 0, std_yr = 0;
	double std_zya = 0, std_zp = 0, std_zr = 0;
	double std_yap = 0, std_yar = 0;
	double std_pr = 0;

	// Mean values in [0, pi] range:
	double mean_yaw = mean.yaw;
	double mean_pitch = mean.pitch;
	double mean_roll = mean.roll;

	// enough information to estimate the covariance?
	if (m_particles.size() < 2) return tuple<Eigen::MatrixXd, Pose3D>{cov, mean };

	// Sum all weight values :
	double W = 0;
	for (const auto& p : m_particles) W += exp(p.log_w);

	for (const auto& p : m_particles)
	{
		double w = exp(p.log_w) / W;

		// Manage 1 PI range:
		double err_yaw = std::abs(p.data.yaw - mean_yaw);
		double err_pitch = std::abs(p.data.pitch - mean_pitch);
		double err_roll = std::abs(p.data.roll - mean_roll);

		double err_x = p.data.x - mean.x;
		double err_y = p.data.y - mean.y;
		double err_z = p.data.z - mean.z;

		vars[0] += std::pow(err_x,2) * w;
		vars[1] += std::pow(err_y,2) * w;
		vars[2] += std::pow(err_z, 2) * w;
		vars[3] += std::pow(err_yaw, 2) * w;
		vars[4] += std::pow(err_pitch, 2) * w;
		vars[5] += std::pow(err_roll, 2) * w;

		std_xy += err_x * err_y * w;
		std_xz += err_x * err_z * w;
		std_xya += err_x * err_yaw * w;
		std_xp += err_x * err_pitch * w;
		std_xr += err_x * err_roll * w;

		std_yz += err_y * err_z * w;
		std_yya += err_y * err_yaw * w;
		std_yp += err_y * err_pitch * w;
		std_yr += err_y * err_roll * w;

		std_zya += err_z * err_yaw * w;
		std_zp += err_z * err_pitch * w;
		std_zr += err_z * err_roll * w;

		std_yap += err_yaw * err_pitch * w;
		std_yar += err_yaw * err_roll * w;

		std_pr += err_pitch * err_roll * w;
	}  // end for it

	// Unbiased estimation of variance:
	cov(0, 0) = vars[0];
	cov(1, 1) = vars[1];
	cov(2, 2) = vars[2];
	cov(3, 3) = vars[3];
	cov(4, 4) = vars[4];
	cov(5, 5) = vars[5];

	cov(1, 0) = cov(0, 1) = std_xy;
	cov(2, 0) = cov(0, 2) = std_xz;
	cov(3, 0) = cov(0, 3) = std_xya;
	cov(4, 0) = cov(0, 4) = std_xp;
	cov(5, 0) = cov(0, 5) = std_xr;

	cov(2, 1) = cov(1, 2) = std_yz;
	cov(3, 1) = cov(1, 3) = std_yya;
	cov(4, 1) = cov(1, 4) = std_yp;
	cov(5, 1) = cov(1, 5) = std_yr;

	cov(3, 2) = cov(2, 3) = std_zya;
	cov(4, 2) = cov(2, 4) = std_zp;
	cov(5, 2) = cov(2, 5) = std_zr;

	cov(4, 3) = cov(3, 4) = std_yap;
	cov(5, 3) = cov(3, 5) = std_yar;

	cov(5, 4) = cov(4, 5) = std_pr;

	return tuple<Eigen::MatrixXd, Pose3D>{cov, mean };
}



Pose3D ParticlesData::getMean(Pose3D & pose)
{
	pose = Pose3D();
	if (m_particles.empty()) return pose;

	m_count = 0.0;
	m_accum_x = 0.0;
	m_accum_y = 0.0;
	m_accum_z = 0.0;
	m_accum_yaw = 0.0;
	m_accum_pitch = 0.0;
	m_accum_roll = 0.0;

	for (auto& p:m_particles) {
		const double w = exp(p.log_w);
		append(p.data,w);
	}
	getAverage(pose);
	return pose;
}

void ParticlesData::append(const Pose3D & p, const double weight)
{
	m_count += weight;
	m_accum_x += weight * p.x;
	m_accum_y += weight * p.y;
	m_accum_z += weight * p.z;
	m_accum_yaw += weight * p.yaw;
	m_accum_pitch += weight * p.pitch;
	m_accum_roll += weight * p.roll;

	/*m_count += 1;
	m_accum_x +=  p.x;
	m_accum_y += p.y;
	m_accum_z +=  p.z;
	m_accum_yaw += p.yaw;
	m_accum_pitch += p.pitch;
	m_accum_roll += p.roll;*/
}

void ParticlesData::append(const Eigen::Matrix3d & rot, const double weight)
{
	//TODO
}

void ParticlesData::getAverage(Pose3D & return_mean)
{
	if (m_count == 0)
		return;
	return_mean.x=m_accum_x / m_count;
	return_mean.y=m_accum_y / m_count;
	return_mean.z=m_accum_z / m_count;
	return_mean.yaw = m_accum_yaw / m_count;
	return_mean.pitch = m_accum_pitch / m_count;
	return_mean.roll = m_accum_roll / m_count;
	return_mean.fromROT3();
}


Pose3D ParticlesData::getLastPose(size_t index)
{
	if (index>=m_particles.size()) {
		throw "---- Exception : outof range when getLastPose ----";
	}
	return m_particles[index].data;
}

void ParticlesData::updateParticleWithNewPose(Pose3D& particlePose, Pose3D  newPose)
{
	particlePose = newPose;
}

pcl::PointCloud<pcl::PointXYZRGB>::Ptr ParticlesData::getParticlesAsPointCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud)
{
	try {
		cloud->points.clear();
		pcl::PointXYZRGB point;
		for (int i = 0; i < m_particles.size();i++) {
			point.x = m_particles[i].data.x;
			point.y = m_particles[i].data.y;
			point.z = m_particles[i].data.z;
			point.r = 255;
			point.g = 0;
			point.b = 0;
			cloud->push_back(point);
		}
		return cloud;
	}
	catch(...){
		throw "oops! there is some wrong of function getParticlesAsPointCloud";
	}
	return nullptr;
	
}

pcl::PointCloud<pcl::PointXYZRGB>::Ptr ParticlesData::getParticlesAsPointCloud(const Particles& shift_particles, pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud)
{
	try {
		cloud->points.clear();
		pcl::PointXYZRGB point;
		for (int i = 0; i < m_particles.size(); i++) {
			point.x = m_particles[i].data.x;
			point.y = m_particles[i].data.y;
			point.z = m_particles[i].data.z;
			point.r = 255;
			point.g = 0;
			point.b = 0;
			cloud->push_back(point);

			point.x = shift_particles[i].data.x;
			point.y = shift_particles[i].data.y;
			point.z = shift_particles[i].data.z;
			point.r = 0;
			point.g = 0;
			point.b = 255;
			cloud->push_back(point);
		}
		return cloud;
	}
	catch (...) {
		throw "oops! there is some wrong of function getParticlesAsPointCloud";
	}
	return nullptr;
}

void ParticlesData::logOutPutAboutParticles(string root)
{
	//guass save
	string predictionMean_output = root + "predictionMean.txt";
	ofstream predictionMeanfout;
	predictionMeanfout.open(predictionMean_output);//
	for (auto& s : guassPoseMean) {
		predictionMeanfout << s.x<<" "<<s.y<<" "<<s.z<<" "<<s.yaw << endl;
	}predictionMeanfout.close();

	string guassPoseCov_output = root + "predictionCov.txt";
	ofstream predictionguassPoseCovfout;
	predictionguassPoseCovfout.open(guassPoseCov_output);//
	for (auto& q : guassPoseCov) {
		predictionguassPoseCovfout << q(0,0) << " " << q(1, 1) << " " << q(2, 2) << " " << q(3, 3) << endl;
	}predictionguassPoseCovfout.close();

	
	//resample save
	string resamplePoseMean_output = root + "resamplePoseMean.txt";
	ofstream resamplePoseMeanfout;
	resamplePoseMeanfout.open(resamplePoseMean_output);//
	for (auto& s : resamplePoseMean) {
		resamplePoseMeanfout << s.x << " " << s.y << " " << s.z << " " << s.yaw << endl;
	}resamplePoseMeanfout.close();

	string resamplePoseCov_output = root + "resamplePoseCov.txt";
	ofstream resamplePoseCovfout;
	resamplePoseCovfout.open(resamplePoseCov_output);//
	for (auto& q : resamplePoseCov) {
		resamplePoseCovfout << q(0, 0) << " " << q(1, 1) << " " << q(2, 2) << " " << q(3, 3) << endl;
	}resamplePoseCovfout.close();

	//shift save
	string shiftPoseMean_output = root + "shiftPoseMean.txt";
	ofstream shiftPoseMeanfout;
	shiftPoseMeanfout.open(shiftPoseMean_output);//
	for (auto& s : shiftPoseMean) {
		shiftPoseMeanfout << s.x << " " << s.y << " " << s.z << " " << s.yaw << endl;
	}shiftPoseMeanfout.close();

	string shiftPoseCov_output = root + "shiftPoseCov.txt";
	ofstream shiftPoseCovfout;
	shiftPoseCovfout.open(shiftPoseCov_output);//
	for (auto& q : shiftPoseCov) {
		shiftPoseCovfout << q(0, 0) << " " << q(1, 1) << " " << q(2, 2) << " " << q(3, 3) << endl;
	}shiftPoseCovfout.close();

	string singleIterateParticlesPF_output = root + "singleIterateParticlesPF.txt";
	ofstream singleIterateParticlesPFfout;
	singleIterateParticlesPFfout.open(singleIterateParticlesPF_output);//
	for (auto& s : singleIterateParticlesPF) {
		singleIterateParticlesPFfout << s.x << " " << s.y << " " << s.z << " " << s.yaw << endl;
	}singleIterateParticlesPFfout.close();

	string singleIterateParticlesSPF_output = root + "singleIterateParticlesSPF.txt";
	ofstream singleIterateParticlesSPFfout;
	singleIterateParticlesSPFfout.open(singleIterateParticlesSPF_output);//
	for (auto& s : singleIterateParticlesSPF) {
		singleIterateParticlesSPFfout << s.x << " " << s.y << " " << s.z << " " << s.yaw << endl;
	}singleIterateParticlesSPFfout.close();
}

void ParticlesData::saveGuassCovarianceAndMean()
{
	//guassPoseMean.push_back(m_particles[10].data);
	guassPoseMean.push_back(get<1>(getCovarianceAndMean()));
	guassPoseCov.push_back(get<0>(getCovarianceAndMean()));
}

void ParticlesData::saveResampleCovarianceAndMean()
{
	//resamplePoseMean.push_back(m_particles[10].data);
	resamplePoseMean.push_back(get<1>(getCovarianceAndMean()));
	resamplePoseCov.push_back(get<0>(getCovarianceAndMean()));


	saveSingleIterateOfAllParticlesPF();
}

void ParticlesData::saveShiftCovarianceAndMean()
{
	//shiftPoseMean.push_back(m_particles[10].data);
	shiftPoseMean.push_back(get<1>(getCovarianceAndMean()));
	shiftPoseCov.push_back(get<0>(getCovarianceAndMean()));

	saveSingleIterateOfAllParticlesSPF();
}

void ParticlesData::saveSingleIterateOfAllParticlesPF()
{
	for (auto& p:m_particles) {
		singleIterateParticlesPF.push_back(p.data);
	}
}

void ParticlesData::saveSingleIterateOfAllParticlesSPF()
{
	for (auto& p : m_particles) {
		singleIterateParticlesSPF.push_back(p.data);
	}
}



